package org.nashua.toughtechs151;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import java.util.TimerTask;
import org.nashua.toughtechs151.dash.Dash;
import org.nashua.toughtechs151.dash.Data;

public class ChocolateChip extends SimpleRobot {
    private Jaguar frontLeft = new Jaguar(RobotMap.JAGUAR_FRONT_LEFT);
    private Jaguar frontRight = new Jaguar(RobotMap.JAGUAR_FRONT_RIGHT);
    private Jaguar backLeft = new Jaguar(RobotMap.JAGUAR_BACK_LEFT);
    private Jaguar backRight = new Jaguar(RobotMap.JAGUAR_BACK_RIGHT);
    private Jaguar pitch = new Jaguar(RobotMap.JAGUAR_SHOOTER_PITCH);
    private Jaguar yaw = new Jaguar(RobotMap.JAGUAR_SHOOTER_YAW);
    private Jaguar shooter = new Jaguar(RobotMap.JAGUAR_SHOOTER);
    private Dash dash;
    private Data data;
    private DigitalOutput led = new DigitalOutput(RobotMap.SPIKE_RELAY);
    private boolean isControllable = true;
    private boolean isAutoDrive = false;
    private double power=0;
    private long time=0;
    private Gyro gyro = new Gyro(1);
    public final int HIGHEST=0, TOP=1, MIDDLE_LEFT=2, MIDDLE=3, MIDDLE_RIGHT=4, BOTTOM=5, LOWEST=6;
    public void robotInit() {
        try {
            final ChocolateChip r=this;
            dash = new Dash(r);
            new java.util.Timer().scheduleAtFixedRate(new TimerTask(){
                public void run() {
                    try {
                        double multiplier = MathUtils.pow(10,2);
                        dash.updatePWM("Input", 1, "Gyro", ""+((int)(gyro.getAngle()*multiplier))/multiplier);
                    } catch (Exception e) {
                        try {
                            dash = new Dash(r);
                        } catch (Exception ex) {
                        }
                    }
                }
            }, 1, 1000);
        } catch (Exception ex) {
            ex.printStackTrace();
        }
        gyro.reset();
    }
    public void autonomous() {
        power = 1;
        frontLeft.set(power);
        backLeft.set(power);
        frontRight.set(power);
        backRight.set(power);
        Timer.delay(1);
        frontLeft.set(0);
        backLeft.set(0);
        frontRight.set(0);
        backRight.set(0);
    }
    public void operatorControl() {
        frontLeft.set(0);
        backLeft.set(0);
        frontRight.set(0);
        backRight.set(0);
        isAutoDrive = false;
        while (isEnabled()&&isOperatorControl()) {
            if (OI.getDriverButton(9)) led.set(true);
            else if(OI.getDriverButton(10)) led.set(false);
            if (OI.getDriverButton(6)) isControllable=true;
            else {
                isControllable=false;
                frontLeft.set(0);
                backLeft.set(0);
                frontRight.set(0);
                backRight.set(0);
            }
            if (isControllable) {
                if (OI.getDriverButton(2)) {power=0.25; time=1000; isAutoDrive=true; isControllable=false;}
                else if (OI.getDriverButton(3)) {power=0.25; time=2000; isAutoDrive=true; isControllable=false;}
                else if (OI.getDriverButton(4)) {
                    power=1;
                    turnTo(HIGHEST);
                    shoot(HIGHEST);
                }
                else if (OI.getDriverButton(7)) {
                    power=0.25;
                    driveTo(60);
                } else if (OI.getDriverButton(5)) {
                    lockOn(HIGHEST,60,0.25,1);
                }
                //System.out.println("isControllable");
                frontLeft.set(-OI.getDriverAxis(2));
                backLeft.set(frontLeft.get());
                frontRight.set(OI.getDriverAxis(4));
                backRight.set(frontRight.get());
            } else if (isAutoDrive) {
                //System.out.println("isAutoDrive");
                frontLeft.set(power);
                backLeft.set(power);
                frontRight.set(-power);
                backRight.set(-power);
                Timer.delay(1);
                frontLeft.set(0);
                backLeft.set(0);
                frontRight.set(0);
                backRight.set(0);
                isAutoDrive=false;
                //System.out.println("isAutoDrive-Ended");
            }
        }
    }
    public boolean driveTo(int inches) {
        String ret = dash.requestInfo("DISTANCE");
        int range = Integer.parseInt(ret);
        if (range==0) return true;
        int distance = range-inches;
        if (Math.abs(distance)<5) return true;
        if (distance<0) power=-power;
        double seconds = (distance*1.0)/(84*power);
        System.out.println("Distance "+distance+" / Drive To "+inches+" / Power "+power+" / Seconds "+seconds);
        frontLeft.set(power);
        backLeft.set(power);
        frontRight.set(-power);
        backRight.set(-power);
        Timer.delay(seconds);
        frontLeft.set(0);
        backLeft.set(0);
        frontRight.set(0);
        backRight.set(0);
        return false;
    }

    private void turnTo(int target) {
        int dis = Integer.parseInt(dash.requestInfo("DISTANCE-T"+target));
        if (dis==0) {System.out.println("invalid distance"); return;}
        int off = Integer.parseInt(dash.requestInfo("OFFSET-T"+target));
        if (Math.abs(off)<=3) {System.out.println("invalid offset"); return;}
        if (off<0) power=-power;
        double angle = ((int)(Math.toDegrees(MathUtils.atan(off*1.0/dis))*100))/100.0;
        double seconds = angle/(136.0*power);
        System.out.println("Distance "+dis+" / Offset "+off+" / Angle "+angle+" / Power "+power+" / Seconds "+seconds);
        if (seconds<=0||seconds==Double.NaN) {System.out.println("invalid length"); return;}
        frontLeft.set(-power);
        backLeft.set(-power);
        frontRight.set(-power);
        backRight.set(-power);
        Timer.delay(seconds);
        frontLeft.set(0);
        backLeft.set(0);
        frontRight.set(0);
        backRight.set(0);
    }

    private void lockOn(int target, int inches, double drivePower, double turnPower) {
        while (isEnabled() && isOperatorControl()) {
            int dis = Integer.parseInt(dash.requestInfo("DISTANCE-T"+target));
            if (dis==0) {
                System.out.println("invalid distance");
                frontLeft.set(0);
                backLeft.set(0);
                frontRight.set(0);
                backRight.set(0);
                for (int i=0;i<3;i++) {
                    // Failed To Identify Target - Wait For 200MS - TRY 3 TIMES
                    Timer.delay(0.2);
                    dis = Integer.parseInt(dash.requestInfo("DISTANCE-T"+target));
                    if (dis!=0) break;
                }
                if (dis==0) return;
            }
            int off = Integer.parseInt(dash.requestInfo("OFFSET-T"+target));
            double angle = ((int)(Math.toDegrees(MathUtils.atan(off*1.0/dis))*100))/100.0;
            double distance = dis-inches;
            System.out.println("Distance "+distance+" / Offset "+off);
            if (Math.abs(off)>3) {
                power=turnPower;
                System.out.println("Angle");
                if (off<0) power=-power;
                frontLeft.set(-power);
                backLeft.set(-power);
                frontRight.set(-power);
                backRight.set(-power);
            } else if (Math.abs(distance)>6) {
                power=drivePower;
                System.out.println("Distance");
                if (distance<0) power=-power;
                frontLeft.set(power);
                backLeft.set(power);
                frontRight.set(-power);
                backRight.set(-power);
            } else {
                System.out.println("Off");
                frontLeft.set(0);
                backLeft.set(0);
                frontRight.set(0);
                backRight.set(0);
            }
            Timer.delay(0.3);
        }
    }

    private void shoot(int target) {
        String sho = dash.requestInfo("SHOOT-T"+target);
        double pi=0, yi=0;
        pi=Integer.parseInt(sho.substring(0,sho.indexOf(","))); // Pitch Angle
        yi=Integer.parseInt(sho.substring(sho.indexOf(",")+1)); // Yaw Angle
        pitch.set(pi);
        yaw.set(yi);
        while (isEnabled()) {
            pi += OI.getShooterAxis(1)*20;
            yi += OI.getShooterAxis(2)*5;
            pi += OI.getShooterAxis(3);
            yi += OI.getShooterAxis(4);
            pi %= 45; // 45 max
            yi %= 5; // 5 max
            pitch.set(pi);
            yaw.set(yi);
            if (OI.getShooterButton(7)) break;
        }
        shooter.set(1);
        Timer.delay(5);
        shooter.set(0);
    }
}